;;; Interface to Chipmunk 2D Physics Engine

(in-package :cp)

(export '(cpv.x cpv.y moment-for-circle cpvrotate make-cpv-array gravity
	  vec cpv cpv+ cpv- cpv= cpvneg cpv* cpvdot +cpvzero+ +infinity+
	  body mass moment pos angle rotation
	  new-body make-circle-shape make-box-shape make-segment-shape
	  name find-in-space friction elasticity collision-type offset
	  space step-simulation add-static-object add-object bodies shapes joints
	  segment-shape start-point end-point thickness
	  circle-shape center radius moment-for-circle
	  polygon-shape vertices moment-for-polygon
	  draw-shape))

(defparameter *chipmunk-initialized* nil)
(defparameter *cpv-size* (ccl::foreign-size :cp<V>ect :bytes))

(eval-when (:compile-toplevel :load-toplevel)
  ;(ccl:use-interface-dir :chipmunk)
  ;(ccl:open-shared-library "/Users/millejoh/Applications/DreamingTree/libChipmunkDynamic.dylib")
  ;; Do this now, since it needs to be done only once per app, and it is a good check to
  ;; see if the shared libraries have loaded.
  (#_cpInitChipmunk)
  (setq *chipmunk-initialized* t))

(defun initarg-member-value (initargs member)
  (let ((idx (position member initargs)))
    (when idx
      (elt initargs (1+ idx)))))

(defmacro define-member-accessors (type-name name member &optional (read-only nil))
  (let* ((accessor-name (intern (format nil "~A" name)))
         (accessor-type (intern (format nil "~A.~A" (case type-name
                                                      (space "CP<S>PACE")
                                                      (body "CP<B>ODY")
                                                      (shape "CP<S>HAPE")
                                                      (circle-shape "CP<C>IRCLE<S>HAPE")
                                                      (segment-shape "CP<S>EGMENT<S>HAPE")
                                                      (polygon-shape "CP<P>OLY<S>HAPE"))
                                        member)
                                :keyword))
         (getter-func (aux:with-unique-names (object)
                        `(defmethod ,accessor-name ((,object ,type-name))
                           (with-slots (ptr) ,object
                             (pref ptr ,accessor-type)))))
         (setter-func (aux:with-unique-names (object nval)
                        `(defmethod (setf ,accessor-name) (,nval (,object ,type-name))
                           (with-slots (ptr) ,object
                             (setf (pref ptr ,accessor-type) (if (cp-object-p ,nval)
								 (ptr ,nval)
								 ,nval))
                             (if (slot-boundp ,object ',accessor-name)
                               (setf (slot-value ,object ',accessor-name) ,nval)))))))
    ;(format t "~A, ~A~%" accessor-name accessor-type)
    (if read-only
	`(,@getter-func)
	`(progn
	   ,getter-func
	   ,setter-func))))

;;;; ---- ;;;;
;;; Class Definitions

(defclass cp-object ()
  ((ptr :reader ptr :initform nil :initarg :ptr)
   (name :accessor name :initarg :name :initform (symbol-name (gensym "CP")))))

(defun cp-object-p (object)
  (subtypep (type-of object) (find-class 'cp-object)))

(defclass vec (cp-object)
  ())
  
(defclass space (cp-object)
  ((bodies :accessor bodies :initarg :bodies :initform nil)
   (gravity :initarg :gravity)
   (shapes :accessor shapes :initarg :shapes :initform nil)
   (joints :accessor joints :initarg :joints :initform nil)
   (time-step :accessor time-step :initarg :time-step :initform (/ 1.0d0 60.0d0))
   (static-bodies :accessor static-bodies :initarg :static-bodies :initform nil)))

(defclass body (cp-object)
  ((mass :initarg :mass)
   (moment :initarg :moment)
   (pos :initarg :position)))

(defclass shape (cp-object)
  ((body :accessor body :initarg :body)
   (elasticity :initarg :elasticity)
   (friction :initarg :friction)
   (collision-type :initarg :collision-type))
  (:default-initargs
   :elasticity 0.8d0
    :friction 1.0d0
    :collision-type 1))

(defclass circle-shape (shape)
  ((radius :initarg :radius)
   (offset :initarg :offset)))

(defclass segment-shape (shape)
  ((thickness :initarg :thickness)
   (start-point :initarg :start-point :reader start-point)
   (end-point :initarg :end-point :reader end-point)))
   
(defclass polygon-shape (shape)
  ((vertices :accessor vertices :initarg :vertices)
   (offset :initarg :offset)))

(defclass constraint (cp-object)
  ())

;;;; Vectors

;;; Re-defining in Lisp because Chipmunk declares them all
;;; in cpVect.h as static inline.  May want to do something
;;; similar here (declare inline, or as macros).


(defconstant +infinity+ most-positive-double-float)

(defun cpv.x (cpv)
  (declare (inline cpv.x))
  (typecase cpv
     (vec (pref (ptr cpv) #>cpVect.x))
     (t (pref cpv #>cpVect.x))))

(defun set-cpv.x (cpv nval)
  (typecase cpv
    (vec (setf (pref (ptr cpv) #>cpVect.x)
	       nval))
    (t (setf (pref cpv #>cpVect.x)
	     nval))))

(defun cpv.y (cpv)
  (declare (inline cpv.y))
  (typecase cpv
    (vec (pref (ptr cpv) #>cpVect.y))
    (t (pref cpv #>cpVect.y))))

(defun set-cpv.y (cpv nval)
  (typecase cpv
    (vec (setf (pref (ptr cpv) #>cpVect.y)
	       nval))
    (t (setf (pref cpv #>cpVect.y)
	     nval))))

(defsetf cpv.x set-cpv.x)
(defsetf cpv.y set-cpv.y)

(defun %make-cpv (x y)
  (let ((a (make-record (:struct :cp<V>ect (x :double-float) (y :double-float)))))
    (setf (cpv.x a) (coerce x 'double-float)
	  (cpv.y a) (coerce y 'double-float))
    a))

(defun cpv (x y)
  (make-instance 'vec :ptr (%make-cpv x y)))

(defun cpv= (v1 v2)
  (and (= (cpv.x v1) (cpv.x v2))
       (= (cpv.y v1) (cpv.y v2))))

(defun cpv+ (v1 v2)
  (cpv (+ (cpv.x v1) (cpv.x v2))
       (+ (cpv.y v1) (cpv.y v2))))

(defun cpvneg (v)
  (cpv (- (cpv.x v)) (- (cpv.y v))))

(defun cpv- (v1 v2)
  (cpv (- (cpv.x v1) (cpv.x v2))
       (- (cpv.y v1) (cpv.y v2))))

(defun cpv* (v s)
  (cpv (* (cpv.x v) s)
       (* (cpv.y v) s)))

(defun cpvdot (v1 v2)
  (+ (* (cpv.x v1) (cpv.x v2))
     (* (cpv.y v1) (cpv.y v2))))

(defun cpvcross (v1 v2)
  (- (* (cpv.x v1) (cpv.y v2))
     (* (cpv.y v1) (cpv.x v2))))

(defun cpvperp (v)
  (cpv (- (cpv.y v)) (cpv.x v)))

(defun cpvrperp (v)
  (cpv (cpv.y v) (- (cpv.x v))))

(defun cpvproject (v1 v2)
  (cpv* v2 (/ (cpvdot v1 v2) (cpvdot v2 v2))))

(defun cpvrotate (v1 v2)
  (cpv (- (* (cpv.x v1) (cpv.x v2))
	  (* (cpv.y v1) (cpv.y v2)))
       (+ (* (cpv.x v1) (cpv.y v2))
	  (* (cpv.y v1) (cpv.x v2)))))

(defun cpvunrotate (v1 v2)
  (cpv (+ (* (cpv.x v1) (cpv.x v2))
	  (* (cpv.y v1) (cpv.y v2)))
       (- (* (cpv.x v1) (cpv.y v2))
	  (* (cpv.y v1) (cpv.x v2)))))

(defparameter +cpvzero+ (cpv 0.0 0.0))

(defmacro with-cpv (name x y &body body)
  `(rlet ((,name :cp<V>ect))
     (progn
       (setf (pref,name :cp<V>ect.x) ,x 
	     (pref ,name :cp<V>ect.y) ,y)
       ,@body)))

(defun make-cpv-array (verts)
  (let ((a (#_calloc (length verts) *cpv-size*)))
    (iterate (for i upfrom 0)
             (for v in verts)
             (after-each (setf (ccl:paref a (:* (:struct #>cpVect)) i)
                               (%make-cpv (first v) (second v)))))
    a))


(defun release-cpv-array (a)
  (#_free a))

(defmacro with-cpv-array (name xy-pairs &body body)
  `(let ((,name (make-cpv-array ,xy-pairs)))
     (unwind-protect
         ,@body
       (release-cpv-array ,name))))

(defmacro cpv-aref (varray idx)
  `(ccl:paref ,varray (:* (:struct #>cpVect)) ,idx))

(defun test-with-cpv0 ()
  (with-cpv vec 0.0d0 -100.0d0
    (format t "(~A, ~A)~%" (pref vec :cp<V>ect.x)
	    (pref vec :cp<V>ect.y))))

(defun test-with-cpv-array0 ()
  (with-cpv-array verts '((-15.0d0 -15.0d0)
                          (-15.0d0 15.0d0)
                          (15.0d0 15.0d0)
                          (15.0d0 -15.0d0))
    verts))

(defun test-cpv-array ()
  (let* ((verts '((-15.0d0 -15.0d0)
                  (-15.0d0 15.0d0)
                  (15.0d0 15.0d0)
                  (15.0d0 -15.0d0)))
         (varray (make-cpv-array verts)))
    (format t "[[~A ~A]~% [~A ~A]~% [~A ~A]~% [~A ~A]]~%"
            (cpv.x (cpv-aref varray 0)) (cpv.y (cpv-aref varray 0))
            (cpv.x (cpv-aref varray 1)) (cpv.y (cpv-aref varray 1))
            (cpv.x (cpv-aref varray 2)) (cpv.y (cpv-aref varray 2))
            (cpv.x (cpv-aref varray 3)) (cpv.y (cpv-aref varray 3)))
    (assert (= (cpv.x (cpv-aref varray 0)) -15.0d0))
    (assert (= (cpv.y (cpv-aref varray 0)) -15.0d0))
    (assert (= (cpv.x (cpv-aref varray 1)) -15.0d0))
    (assert (= (cpv.y (cpv-aref varray 1)) 15.0d0))
    (assert (= (cpv.x (cpv-aref varray 2)) 15.0d0))
    (assert (= (cpv.y (cpv-aref varray 2)) 15.0d0))
    (assert (= (cpv.x (cpv-aref varray 3)) 15.0d0))
    (assert (= (cpv.y (cpv-aref varray 3)) -15.0d0))))
    

;;;; ---- ;;;;
;;;; Bounding boxes

(defmacro cp-bb.l (bb)
  `(pref, bb :cp<BB>.l))

(defmacro cp-bb.r (bb)
  `(pref, bb :cp<BB>.r))

(defmacro cp-bb.t (bb)
  `(pref, bb :cp<BB>.t))

(defmacro cp-bb.b (bb)
  `(pref, bb :cp<BB>.b))

(defun cp-bb (left right top bottom)
  (let ((a (make-record (:struct :cp<BB> 
                                 (l :double-float) (r :double-float) 
                                 (t :double-float) (b :double-float)))))
    (setf (cp-bb.l a) (coerce left 'double-float)
	  (cp-bb.r a) (coerce right 'double-float)
	  (cp-bb.t a) (coerce top 'double-float)
          (cp-bb.b a) (coerce bottom 'double-float))
    a))

(defun bb-intersects (bb1 bb2)
  (let ((bb1.l (pref bb1 :cp<BB>.l))
	(bb1.r (pref bb1 :cp<BB>.r))
	(bb1.b (pref bb1 :cp<BB>.b))
	(bb1.t (pref bb1 :cp<BB>.t))
	(bb2.l (pref bb2 :cp<BB>.l))
	(bb2.r (pref bb2 :cp<BB>.r))
	(bb2.b (pref bb2 :cp<BB>.b))
	(bb2.t (pref bb2 :cp<BB>.t)))
    (and (<= bb1.l bb2.r)
	 (<= bb2.l bb1.r)
	 (<= bb1.b bb2.t)
	 (<= bb2.b bb1.t))))

(defun bb-contains-bb (bb other)
  (let ((bb.l (pref bb :cp<BB>.l))
	(bb.r (pref bb :cp<BB>.r))
	(bb.b (pref bb :cp<BB>.b))
	(bb.t (pref bb :cp<BB>.t))
	(other.l (pref other :cp<BB>.l))
	(other.r (pref other :cp<BB>.r))
	(other.b (pref other :cp<BB>.b))
	(other.t (pref other :cp<BB>.t)))
    (and (< bb.l other.l)
	 (> bb.r other.r)
	 (< bb.b other.b)
	 (> bb.t other.t))))

(defun bb-contains-vec (bb vec)
  (let ((bb.l (pref bb :cp<BB>.l))
	(bb.r (pref bb :cp<BB>.r))
	(bb.b (pref bb :cp<BB>.b))
	(bb.t (pref bb :cp<BB>.t))
	(vec.x (cpv.x vec))
	(vec.y (cpv.y vec)))
    (and (< bb.l vec.x)
	 (> bb.r vec.x)
	 (< bb.b vec.y)
	 (> bb.t vec.y))))


;;;; ---- ;;;;
;;; Chipmunk Spaces
(defparameter *the-space* nil)

(define-member-accessors space iterations iterations)
(define-member-accessors space elastic-iterations elastic<I>terations)
(define-member-accessors space gravity gravity)
(define-member-accessors space timestamp stamp t)

(defmethod initialize-instance :after ((self space) &rest initargs)
  (declare (ignore initargs))
  (with-slots (ptr) self
    (setf ptr (#_cpSpaceNew))
    (#_cpResetShapeIdCounter))
  (unless (slot-boundp self 'name)
    (setf (slot-value self 'name) (gensym "CP")))
  (when (slot-boundp self 'gravity)
    (setf (gravity self) (slot-value self 'gravity)))
  (ccl:terminate-when-unreachable self))

(defmethod ccl:terminate ((self space))
  (with-slots (ptr static-bodies) self
    (#_cpSpaceFreeChildren ptr)
    (#_cpSpaceFree ptr)
    (when static-bodies
      (dolist (sb static-bodies)
        (#_cpBodyFree sb)))))

(defun step-simulation (s &optional (iterations 1))
  (dotimes (i iterations)
    (#_cpSpaceStep (ptr s) (time-step s))))

(defmethod add-object ((self space) (shape shape))
  (with-slots (ptr shapes) self
    (push shape shapes)
    (#_cpSpaceAddShape ptr (ptr shape))))

(defmethod add-object ((self space) (body body))
  (with-slots (ptr bodies) self
    (push body bodies)
    (#_cpSpaceAddBody ptr (ptr body))))

(defmethod add-object ((self space) (joint constraint))
  (with-slots (ptr joints) self
    (push joint joints)
    (#_cpSpaceAddConstraint ptr (ptr joint))))

(defmethod add-static-object ((self space) (shape shape))
  (with-slots (ptr shapes) self
    (push shape shapes)
    (#_cpSpaceAddStaticShape ptr (ptr shape))))

(defmethod add-static-object ((self space) (body body))
  (with-slots (ptr static-bodies) self
    (push body static-bodies)))

(defmethod find-in-space ((self space) (name string))
  (ignore-errors 
    (with-slots (shapes bodies joints) self
      (find name (append shapes bodies joints) :test #'string= :key #'name))))

;;;; ---- ;;;;
;;; cpBody
;;;

(defun new-body (mass moment &optional (pos (cpv 0 0)))
  (make-instance 'body :mass mass :moment moment :position pos))

(defgeneric reset-forces (obj)
  (:documentation "Zero the forces on a body."))

(defgeneric apply-force (obj force offset)
  (:documentation "Apply a force (in world coord) to a body at a point
                  relative to the center of gravity (also in world coordinates)."))
(defgeneric apply-damped-spring (body1 body2 anchor1 anchor2 rlen k damp dt)
  (:documentation "Apply a damped spring force between two bodies."))

(declaim (inline pos mass moment))

(define-member-accessors body mass m)
(define-member-accessors body mass-inv m_inv t)
(define-member-accessors body moment i)
(define-member-accessors body moment-inv i_inv t)
(define-member-accessors body pos p)
(define-member-accessors body velocity v)
(define-member-accessors body force f)
(define-member-accessors body angle a t)
(define-member-accessors body angular-velocity w)
(define-member-accessors body rotation rot t)
(define-member-accessors body torque t)

(defmethod shared-initialize :before ((self body) slots &rest initargs)
  (declare (ignore slots))
  (let ((mass-arg (initarg-member-value initargs :mass))
        (moment-arg (initarg-member-value initargs :moment))
        (pos-arg (initarg-member-value initargs :position)))
    (unless (and mass-arg moment-arg)
      (error "Must specify at least mass (~A), and moment (~A) when creating a body." mass-arg moment-arg))
    (setf (slot-value self 'ptr) (#_cpBodyNew (coerce mass-arg 'double-float) 
                                              (coerce moment-arg 'double-float)))
    (when pos-arg (setf (pos self) pos-arg))))


(defmethod apply-impulse ((obj body) force offset)
  (let* ((v (velocity obj))
         (m_inv (mass-inv obj))
         (i_inv (moment-inv obj)))
    (setf (velocity obj) (cpv+ v (cpv* force m_inv)))
    (setf (angular-velocity obj) (* i_inv (cpvcross offset force)))))


(defmethod reset-forces ((obj body))
  (with-slots (ptr) obj
    (#_cpBodyResetForces ptr)))

(defmethod apply-force ((obj body) force offset)
  (#_cpBodyApplyForce (ptr obj) force offset))

(defmethod apply-damped-spring ((body1 body) (body2 body) anchor1 anchor2 rlen k damp dt)
  (#_cpApplyDampedSpring (ptr body1) (ptr body2) anchor1 anchor2 rlen k damp dt))

;;;; ---- ;;;;
;;; cpShape
;;;

(defmethod shared-initialize :after ((self shape) slots &rest initargs)
  (declare (ignore slots))
  (let ((elasticity (initarg-member-value initargs :elasticity))
        (friction (initarg-member-value initargs :friction))
        (collision-type (initarg-member-value initargs :collision-type))
        (body (initarg-member-value initargs :body)))
    (when (slot-boundp self 'ptr)
      (setf (elasticity self) (or (coerce elasticity 'double-float) (slot-value self 'elasticity))
            (friction self) (or (coerce friction 'double-float) (slot-value self 'friction))
            (collision-type self) (or collision-type (slot-value self 'collision-type))
            (body self) (or body (slot-value self 'body))))))


(defmacro with-bbox (var shape &body body)
  `(rlet ((,var :cp<BB>))
     (#_cpShapeCacheBB ,var (shape ,shape))
     ,@body))

(define-member-accessors shape collision-type collision_type)
(define-member-accessors shape collision-group group)
(define-member-accessors shape layer layers)
(define-member-accessors shape elasticity e)
(define-member-accessors shape friction u)
(define-member-accessors shape surface-velocity surface_v)
(define-member-accessors shape shape-body body)

(defmethod pos ((obj shape))
  (pos (body obj)))

(defmethod rotation ((obj shape))
  (rotation (body obj)))

(defmethod pos-x ((obj shape))
  (cpv.x (pos (body obj))))

(defmethod pos-y ((obj shape))
  (cpv.y (pos (body obj))))

;;;;
;; Circles
;;;;

(defmethod shared-initialize :before ((object circle-shape) slots &rest initargs)
  (declare (ignore slots))
  (let ((body (initarg-member-value initargs :body))
        (radius (initarg-member-value initargs :radius))
        (offset (initarg-member-value initargs :offset)))
    (unless (and body radius offset)
      (error "Must specify a body (~A), radius (~A), and offset (~A) when creating a circle shape."
             body
             radius
             offset))
    (setf (slot-value object 'ptr) 
          (#_cpCircleShapeNew (ptr body) (coerce radius 'double-float) (ptr offset))))
  object)

(define-member-accessors circle-shape center c)
(defmethod radius ((self circle-shape))
  (#_cpCircleShapeGetRadius (ptr self)))
(defmethod (setf radius) (new-radius (circle circle-shape))
  (#_cpCircleShapeSetRadius (ptr circle) new-radius))


(defun moment-for-circle (mass r-inner r-outer offset)
  "Calculate the moment of inertia for a circle.  Arguments are similar to cpCircleShapeInit().
  Offfset is the offset of the center from the center of gravity of the rigid body."
  ;; A good boy would make sure parameters are correctly cast to cpFloats and cpVects.
  (#_cpMomentForCircle (coerce mass 'double-float) 
                       (coerce r-inner 'double-float)
                       (coerce r-outer 'double-float)
                       (ptr offset)))

(defun make-circle-shape (mass r-outer 
			  &key (moment nil) (r-inner 0.0d0) (center +cpvzero+)
			  (pos +cpvzero+) (elasticity 0.1d0) (friction 0.1d0))
  (let* ((moment (or moment (moment-for-circle mass r-inner r-outer center)))
	 (body (new-body mass moment pos)))
    (make-instance 'circle-shape :body body
		   :elasticity elasticity
		   :radius r-outer
		   :offset center
		   :friction friction)))
;;;;
;; Segments
;;;;

(defmethod shared-initialize :before ((object segment-shape) slots &rest initargs)
  (declare (ignore slots))
  (let ((start (initarg-member-value initargs :start-point))
        (end (initarg-member-value initargs :end-point))
        (thickness (initarg-member-value initargs :thickness))
        (body (initarg-member-value initargs :body)))
    (unless (and start end thickness body)
      (error "Must specify a body (~A), start point (~A), end point (~A), and thickness (~A)
for a segment shape."
             body
             start
             end
             thickness))
    (setf (slot-value object 'ptr) (#_cpSegmentShapeNew (ptr body) 
                                                        (ptr start)
                                                        (ptr end)
                                                        (coerce thickness 'double-float))))
  object)

(define-member-accessors segment-shape start-point a t)
(define-member-accessors segment-shape end-point b t)
(define-member-accessors segment-shape thickness r)

(defun make-segment-shape (start-point end-point &key (mass +infinity+) (moment +infinity+)
			   (elasticity 0.1d0) (friction 0.1d0) (thickness 0.1d0))
  (let* ((pos start-point)
	 (body (new-body mass moment pos)))
    (make-instance 'segment-shape :body body :start-point start-point :end-point end-point
		   :thickness thickness
		   :elasticity elasticity
		   :friction friction)))
		
;;;;
;; Polygon shapes
;;;;

(defmethod shared-initialize :before ((self polygon-shape) slots &rest initargs)
  (declare (ignore slots))
  (let ((vertices (initarg-member-value initargs :vertices))
        (offset (initarg-member-value initargs :offset))
        (body (initarg-member-value initargs :body)))
    (unless (and vertices offset body)
      (error "Must specify a body (~A), vertex list (~A), and offset (~A)
             for a polygon shape." body vertices offset))
    (setf (slot-value self 'ptr) (#_cpPolyShapeNew (ptr body)
                                                   (length vertices)
                                                   (make-cpv-array vertices)
                                                   (ptr offset)))
    self))

(define-member-accessors polygon-shape vert-ptr verts t)

(defun moment-for-polygon (mass vertex-list offset)
  (let ((verts (make-cpv-array vertex-list)))
    (#_cpMomentForPoly (coerce mass 'double-float) (length vertex-list) verts (ptr offset))))


(defun make-box-shape (&key (mass 1.0d0) (width 1.0d0) (height 1.0d0) (moment nil)
                            (offset +cpvzero+) (pos +cpvzero+) (elasticity 0.1d0) 
                            (friction 0.1d0))
  (let* ((w (/ width 2.0d0))
         (-w (- w))
         (h (/ height 2.0d0))
         (-h (- h))
         (verts `((,-w ,-h)
		  (,-w ,h)
		  (,w ,h)
		  (,w ,-h)))
         (body (new-body mass (or moment (moment-for-polygon mass verts offset)) pos)))
    (make-instance 'polygon-shape
		   :body body
		   :vertices verts
		   :offset offset
		   :elasticity elasticity :friction friction)))

(defun test-polyshape-verts ()
  (let* ((poly-body (make-instance 'body
                      :mass 1.0
                      :moment (moment-for-polygon 1.0 '((-15 -15)
                                                        (-15 15)
                                                        (15 15)
                                                        (15 -15))
                                                  +cpvzero+)
                      :position (cpv -220 240)))
         (poly-shape (make-instance 'polygon-shape 
                       :body poly-body
                       :vertices '((-15 -15)
                                   (-15 15)
                                   (15 15)
                                   (15 -15))
                       :offset +cpvzero+
                       :elasticity 0.1d0 :friction 1.5d0)))
    (rlet ((v1 (:struct #>cpVect))
           (v2 (:struct #>cpVect)))
      (#_cpPolyShapeGetVert v1 (ptr poly-shape) 0)
      (#_cpPolyShapeGetVert v2 (ptr poly-shape) 100)
      (format t "~A vertices.  v1=(~A,~A)   v2=(~A,~A)~%" (#_cpPolyShapeGetNumVerts (ptr poly-shape))
              (cpv.x v1) (cpv.y v1) (cpv.x v2) (cpv.y v2))
      (#_cpPolyShapeGetVert v1 (ptr poly-shape) 10))))

(defun test-poly-validate (verts)
  (format t "cpPolyValidate says... ~A~%" (#_cpPolyValidate (make-cpv-array verts) (length verts)))
  (dotimes (i (length verts) t)
    (let* ((a (cpv (car (nth i verts)) (cadr (nth i verts))))
           (i2 (mod (+ i 1) (length verts)))
           (i3 (mod (+ i 2) (length verts)))
           (b (cpv (car (nth i2 verts)) (cadr (nth i2 verts))))
           (c (cpv (car (nth i3 verts)) (cadr (nth i3 verts)))))
      (format t "~A " (cpvcross (cpv- b a) (cpv- c b)))
      (when (> (cpvcross (cpv- b a) (cpv- c b)) 0.0d0)
        (return nil)))))
           
                         
           
;;;;
;;; cpJoint
;;;;

;;;; ---- ;;;;
;;; Collision Callbacks

(defcallback default-collision-callback (#>cpShape a #>cpShape b #>cpContact contacts :unsigned ncontacts
                                                   :double-float normal-coef :address data :unsigned)
  (format t "~A contact~:P between ~A and ~A.~%" ncontacts a b))

;;;;
;;; Debugging routines
;;;;

;;; These assume the GL context has been set, horrible things will happen if it is not!

(defgeneric draw-shape (shape))

(defmethod draw-shape ((shape segment-shape))
  (let* ((pos (pos shape))
         (rot (rotation shape))
         (start (start-point shape))
         (end (end-point shape))
         (a (cpv+ pos (cpvrotate start rot)))
         (b (cpv+ pos (cpvrotate end rot))))
    (gl:with-gl #$GL_LINES
      (#_glVertex2d (cpv.x a) (cpv.y a))
      (#_glVertex2d (cpv.x b) (cpv.y b)))))

(defmethod draw-shape ((shape circle-shape))
  (let* ((pos (pos shape))
         (rot (rotation shape))
         (angle (angle (body shape)))
         (center (center shape))
         (radius (radius shape))
         (c (cpv+ pos (cpvrotate center rot)))
         (segs 15) ; Increase this to have smoother circles
         (coef (/ (* 2.0d0 PI) segs)))
;    (format t "Circle pos (~A,~A), rot (~A,~A), center (~A,~A), c (~A,~A)~%" (cpv.x pos) (cpv.y pos)
;            (cpv.x rot) (cpv.y rot) (cpv.x center) (cpv.y center)  (cpv.x c) (cpv.y c))
    (gl:with-gl #$GL_LINE_STRIP
      (dotimes (n (1+ segs))
        (let ((rads (* n coef)))
          (let ((x (+ (* radius (cos (+ rads angle))) (cpv.x c)))
                (y (+ (* radius (sin (+ rads angle))) (cpv.y c))))
            ;(format t "Vertex @ (~A, ~A)~%" x y)
            (#_glVertex2d x y))))
      (#_glVertex2d (cpv.x c) (cpv.y c)))))

(defmethod draw-shape ((shape polygon-shape))
  (let* ((pos (pos shape))
         (rot (rotation shape))
         (verts (vertices shape)))
    ;(format t "Drawing polygon ~A at (~A, ~A).~%" shape (cpv.x pos) (cpv.y pos))
    (gl:with-gl #$GL_LINE_LOOP
      (dolist (v verts)
        (let ((v0 (cpv+ pos (cpvrotate (apply #'cpv v) rot))))
          ;(format t "|Rotation:(~A,~A) Vertex:(~A, ~A)|  " (cpv.x rot) (cpv.y rot) (cpv.x v0) (cpv.y v0))
          (#_glVertex2d (cpv.x v0) (cpv.y v0)))))))
  
;;;; ---- ;;;;
;;; Some UI code for editing and creating chipmunk objects

(defclass space-editor (ns:ns-window-controller)
  ((space :accessor space :initarg :space :initform (make-instance 'space)))
  (:metaclass ns:+ns-object))

(objc:defmethod #/init ((wc space-editor))
  ;; Lisp slots are not set up yet when we are called.
  (let ((self (#/initWithWindowNibName: wc #@"SpaceEditor")))
    (format t "Window controller ~A~%" self)
    self))

(objc:defmethod (#/windowDidLoad :void) ((wc space-editor))
  (#/makeKeyAndOrderFront: (#/window wc) wc))

(objc:defmethod (#/windowWillClose: :void) ((wc space-editor) notification)
  (declare (ignore notification))
  (#/autorelease wc))

(objc:defmethod (#/valueForKey: :id) ((wc space-editor) key)
  (format t "~A~%" key))

(defun edit-space (space)
  (gui::execute-in-gui #'(lambda ()
                           (let ((wc (make-instance 'space-editor
                                       :space space)))
                             (#/showWindow: wc nil)))))


;;;; ---- ;;;;
;;; Some setup functions for debugging Chipmunk code.
;;;

(defun create-simple-space ()
  (let* ((space (make-instance 'space :gravity (cpv 0.0 -100.0))))
    space))

(defun create-test-space01 ()
  (let* ((space (make-instance 'space :gravity (cpv 0.0 -100.0)))
         (bod (make-instance 'body :mass 1.0 :moment 1.0 :position (cpv 0 100))))
    (add-object space bod)
    space))
  
(defun create-test-space ()
  (let* ((bod (make-instance 'body :mass 1.0 :moment 1.0 :position (cpv 0 100)))
         (circ (make-instance 'circle-shape :body bod :radius 10.0 :offset +cpvzero+ :name "Circle"))
         (sbod (make-instance 'body :mass +infinity+ :moment +infinity+))
         (floor (make-instance 'segment-shape :body sbod :start-point (cpv -10 0) :end-point (cpv 10 0) :thickness 1))
         (space (make-instance 'space :gravity (cpv 0.0 -100.0))))
    (add-static-object space sbod)
    (add-static-object space floor)
    (add-object space bod)
    (add-object space circ)
    space))

(defun run-test-sim ()
  (let* ((bod (make-instance 'body :mass 1.0 :moment 1.0 :position (cpv 0 100)))
         (circ (make-instance 'circle-shape :body bod :radius 10.0 :offset +cpvzero+ :name "Circle"))
         (sbod (make-instance 'body :mass +infinity+ :moment +infinity+))
         (floor (make-instance 'segment-shape :body sbod :start-point (cpv -10 0) :end-point (cpv 10 0) :thickness 1))
         (space (make-instance 'space :gravity (cpv 0 -100))))
    (flet ((report-pos (object)
             (format t "~A at (~A, ~A)~%" (name object) (cpv.x (pos object)) (cpv.y (pos object)))))
      (add-static-object space sbod)
      (add-static-object space floor)
      (add-object space bod)
      (add-object space circ)
      (report-pos circ)
      (step-simulation space 1)
      (report-pos circ)
      (dotimes (i 100)
        (step-simulation space 1)
        (format t "Timestamp=~A   " (pref (ptr space) :cp<S>pace.stamp))
        (report-pos circ))
      space)))